#include "goal_flyto.h"

goal_flyto(LLHvector coord) {

    dest = LLHtoNED(coord, helicopter.get_reference_point());

} //end constructor

/*!@brief Sends the command to flyto a position.
 *
 * Also begins a timeout clock to make sure the autopilot
 * got the message. 
 */
activate() {

    wating_for_ack = true;
    ack_timeout = time() + timeout_interval;

    helicopter.flyto(dest);

} //end activate

/*!@brief Checks the timeout for the flyto responce and that we have not exceded our time.
 * 
 * This goal will replan on the event that the acknowlage from the autopilot is not receved.
 * It will fail if we exceed our allotted time interval to get to our position.
 */
process() {

    set_status(G_active);

    //check the timeout
    if(waiting_for_ack && ack_timeout >= time()) {

        //reactivate to send the new message
        activate();

    } //end if 

    NEDvector current = helicopter.get_current_position();
    NEDvector delta;

    delta.north = dest.north - current.north;
    delta.east = dest.east - current.east;
    delta.down = dest.down - current.down;

    //if we have made it to our position then we are finished
    if( delta.north < fudge_factor &&
        delta.east  < fudge_factor &&
        delta.down  < fudge_factor) {

        set_status(G_complete);
    } //end if

    //check to see if we are making progress toward our goal 
    else if(arrival_time > time()) {

        set_status(G_failed);

    } //end else if
        
    return get_status(); 

} //end process
